﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NIP.ControllerInterface;
using System.Threading;
using System.Diagnostics;
using NIP.DynamicControl;
using NIP.DynamicControl.Environ;
using NIP.DynamicControl.Constants;



namespace NIP.DynamicControl.PID
{
    public class JointPID
    {
        //controllerObject.MoveTorque((byte)Axis.BaseOrTorso , (int)1300); //1200 is ccw, 1400 is cw MR 28/7
        //controllerObject.MoveTorque((byte)Axis.Shoulder , (int)1300); //1200 is up, 1400 is down MR 28/7
        //controllerObject.MoveTorque((byte)Axis.Elbow , (int)1300); //1200 is down, 1400 is up MR 28/7
        //controllerObject.MoveTorque((byte)Axis.WristPitch , (int)1300); //1200 is down, 1600 is up MR 28/7
        //controllerObject.MoveTorque((byte)Axis.WristRoll , (int)1300); //1200 is ccw, 1400 is cw, MR 28/7
        //private Position currentPos;
        public PVA CurrentPos;
        //public Position desiredPos;
        public PathPoint DesiredPos;
        //private Position errorPos;
        public PathPoint ErrorPos;
        //private Position lastPos;
        public PathPoint LastPos;
        public PathPoint tempPos;
        private double[] tempJoints;
        
        private ControllerInterface.UsbcController controllerObject;
        private int PWM;
        private int errorHistLength;
        private int[] PWMarray;
        private int[] jointDir;
        private int[] jointPWM;
        private int[] holdPWM;
        private int[] maxGain;
        private int joint;
        private int errorCount;
        private int tuningJoint;
        private double[] kp;
        private double[] ki;
        private double[] kd;
        private double[,] errorHist;
        private double dt;
        private double defaultdt;
        private double[] proportional;
        private double[] integral;
        private double[] derivative;
        private double[] kt;
        private double ktDefault;
        private int[] gain;
        private volatile bool _shouldStop;
        private volatile bool _active;
        private volatile bool _sleeping;
        private Object thisLock;
        private DateTime loopEndTime;
        private DateTime loopStartTime;
        private TimeSpan loopDuration;
        public bool InMotion;
        public bool tempMotion;
        private double _stictComp;
        public bool stictComp
        {
            get
            {
                return Convert.ToBoolean(_stictComp);
            }
            set
            {
                _stictComp = Convert.ToDouble(value);
            }
        }
        public double[,] jointMotion;



        public object locker = new object();

        //public bool OpenGripper;
        //public bool CloseGripper;
        //public bool GripClosed;
            
        public JointPID(ControllerInterface.UsbcController USBCObject)
        {
            controllerObject = USBCObject;
            thisLock = new Object();
            _shouldStop = false;
            _active = false;
            _sleeping = false;
            errorHistLength = 10;
            //currentPos = new Position();
            CurrentPos = new PVA();
            ErrorPos = new PathPoint();
            LastPos = new PathPoint();
            tempPos = new PathPoint();
            //errorPos = new Position();
            //lastPos = new Position();
            tuningJoint = 0;
            //desiredPos = controllerObject.GetCurrentPosition();
            PWM = 1300;
            PWMarray = new int[USBC.TotalAxes];
            jointDir = new int[USBC.TotalAxes] { -1, 1, -1, -1, 1, 0, 0, 0 };
            jointPWM = new int[USBC.TotalAxes] { 150, 150, 150, 200, 100, 0, 0, 0 };
            holdPWM = new int[USBC.TotalAxes] { 1297, 1297, 1297, 1310, 1297, 0, 0, 0 };
            maxGain = new int[USBC.TotalAxes] { 225, 225, 300, 225, 255, 0, 0, 0 };
           
            
            errorHist = new double[5,10];
            jointMotion = new double[5, 2];
            dt = 0.02;
            defaultdt = dt;
            InMotion = true;
            tempMotion = false;
            tempJoints = new double[USBC.TotalAxes];
            ktDefault = 0.015;
            //kp = new double[USBC.TotalAxes] { 0.0192, 0.025, 0.0183, 0.015, 0.0095, 0, 0, 0 };
            //ki = new double[USBC.TotalAxes] { 0.0, 0.00014, 0.00012, 0.0015, 0.0, 0, 0, 0 };
            //kd = new double[USBC.TotalAxes] { 0.0044, 0.0063, 0.003, 0.0035, 0.002, 0, 0, 0 };
            kp = new double[USBC.TotalAxes] { PIDconst.J0kp, PIDconst.J1kp, PIDconst.J2kp, PIDconst.J3kp, PIDconst.J4kp, 0, 0, 0 };
            ki = new double[USBC.TotalAxes] { PIDconst.J0ki, PIDconst.J1ki, PIDconst.J2ki, PIDconst.J3ki, PIDconst.J4ki, 0, 0, 0 };
            kd = new double[USBC.TotalAxes] { PIDconst.J0kd, PIDconst.J1kd, PIDconst.J2kd, PIDconst.J3kd, PIDconst.J4kd, 0, 0, 0 };
            kt = new double[USBC.TotalAxes] { ktDefault, ktDefault, ktDefault, ktDefault, ktDefault, 0, 0, 0 };
            stictComp = false;
            proportional = new double[USBC.TotalAxes];
            integral = new double[USBC.TotalAxes];
            derivative = new double[USBC.TotalAxes];
            gain = new int[USBC.TotalAxes];
            DesiredPos = new PathPoint(controllerObject.GetCurrentPosition()); 
            //OpenGripper = false;
            //CloseGripper = false;
            Thread.Sleep(100);
            //GripClosed = _GripClosed;
            Thread.Sleep(100);
            Console.WriteLine("Joint PID's created");
            
        }
        

        public void SetGains(RobotControlEventArgs args)
        {
            //tempJoints = desiredPos.Jointd;
            //tempJoints[args.joint] = args.Angle[args.joint];
            while(_sleeping == false);
            kp[args.joint] = args.kp;
            ki[args.joint] = args.ki;
            kd[args.joint] = args.kd;
            //desiredPos.Jointd = tempJoints;
            //DesiredPos.setJoint(args.joint, args.Angle[args.joint]);
            holdPWM[args.joint] = args.holdPWM;
            //tuningJoint = args.joint;
            //Debug.Write("SetPID: J" + args.joint);
            //Debug.Write(" HoldPWM:" + args.holdPWM +":" + holdPWM[args.joint]);
            //Debug.Write(" Angle:" + args.Angle[args.joint]+ ":" + DesiredPos.getJoint(args.joint));
            //Debug.Write(" Angles:" + DesiredPos.getJoint(0) + "," + DesiredPos.getJoint(1) + "," + DesiredPos.getJoint(2) + "," + DesiredPos.getJoint(3) + "," + DesiredPos.getJoint(4));
            //Debug.Write(" kp:" + args.kp + ":" + kp[args.joint]);
            //Debug.Write(" ki:" + args.ki + ":" + ki[args.joint]);
            //Debug.WriteLine(" kd:{0:N7}:{1:N7}", args.kd, kd[args.joint]);

            


        }

        

        //public void setGripper()
        //{
        //    GripClosed = _GripClosed;
        //    if (OpenGripper && GripClosed)
        //    {
        //        OpenGripper = false;
        //        while(!controllerObject.OpenGripper());
        //        Thread.Sleep(100);
                
                
        //    }
        //    else if (CloseGripper && !GripClosed)
        //    {
        //        CloseGripper = false;
        //        while (!controllerObject.CloseGripper()) ;
        //        Thread.Sleep(100);
                

        //    }
        //    else
        //    {
        //        CloseGripper = false;
        //        OpenGripper = false;
        //    }
        //    GripClosed = _GripClosed;
        //}


        
        public void RequestStop()
        {
            _shouldStop = true;
            controllerObject.Control((byte)Axis.All, false);
            //Thread.Sleep(1000);
            controllerObject.Control((byte)Axis.All, true);
        }

        public void RequestStart()
        {
            _shouldStop = false;
        }

        public bool IsActive()
        {
            return _active;
        }

                  
        public bool PIDinit()
        {
            _active = true;
            loopEndTime = DateTime.Now;
            for (int i = 0; i < 10; i++)
            {
                CurrentPos.update(controllerObject.GetCurrentPosition());
                Thread.Sleep(50);
            }
            LastPos = CurrentPos.get();
            Thread.Sleep(200);
            return true;
        }

        public bool shouldStop()
        {
            if (_shouldStop)
            {
                //while (!controllerObject.Control((byte)'&', false)) ;
                _active = false;

                //controllerObject.CloseTorque();
                //Console.WriteLine("Torque Closed");
                while (_shouldStop)
                {
                    Thread.Sleep(100);
                }
                //! NB HL MAJOR CHANGE!!!
                //while (controllerObject.Control((byte)'&', false)) ;
                controllerObject.Control((byte)Axis.All, true);
                _active = true;
            }
            
            return true;
        }

        public bool setDesiredPos(PathPoint desiredPos)
        {
            DesiredPos = desiredPos;
            return true;
        }

        public bool getCurrPos()
        {
            loopStartTime = DateTime.Now;
            //currentPos = controllerObject.GetCurrentPosition();
            //CurrentPos.update(currentPos);
            CurrentPos.update(controllerObject.GetCurrentPosition());
            //Debug.Write(String.Format("PosXYZ{0,6:N2}, {1,6:N2}, {2,6:N2} ", CurrentPos.PVAhist[2].pos[0], CurrentPos.PVAhist[2].pos[1], CurrentPos.PVAhist[2].pos[2]));
            //Debug.Write(String.Format("VelXYZ{0,6:N2}, {1,6:N2}, {2,6:N2} ", CurrentPos.PVAhist[2].vel[0], CurrentPos.PVAhist[2].vel[1], CurrentPos.PVAhist[2].vel[2]));
            //Debug.WriteLine(String.Format("AccXYZ{0,6:N2}, {1,6:N2}, {2,6:N2} ", CurrentPos.PVAhist[2].acc[0], CurrentPos.PVAhist[2].acc[1], CurrentPos.PVAhist[2].acc[2]));
            return true;
        }

        public bool calculateGains()
        {
            //Debug.Write(String.Format("T:{0,3:N0} ", loopStartTime.Millisecond));
            
            //PathPoint tempPos = CurrentPos.get();
            tempPos = CurrentPos.get();
            for (joint = 0; joint < 5; joint++)
            {
                //errorPos.Joint[joint] = robotObject.PathPlanningObject.pathPoints[1].Joint[joint] - currentPos.Joint[joint];
                if (DesiredPos == null)
                    ErrorPos.setJoint(joint, 0);
                else
                {
                    //jointMotion[joint,0] = position error;
                    jointMotion[joint, 0] = (DesiredPos.getJoint(joint) - tempPos.getJoint(joint));
                    ErrorPos.setJoint(joint, jointMotion[joint, 0]);
                }
                proportional[joint] = kp[joint] * ErrorPos.getJoint(joint);
                //integral[joint] = (integral[joint] + (errorPos.Joint[joint] * dt));
                integral[joint] -= errorHist[joint, 0];
                for (errorCount = 0; errorCount < (errorHistLength - 1); errorCount++)
                {
                    errorHist[joint, errorCount] = errorHist[joint, errorCount + 1];
                }
                errorHist[joint, (errorHistLength - 1)] = (ErrorPos.getJoint(joint) * dt);
                integral[joint] += errorHist[joint, (errorHistLength - 1)];
                //jointMotion[joint,1] = position derivative;
                jointMotion[joint, 1] = kd[joint] * ((tempPos.getJoint(joint) - LastPos.getJoint(joint)) / dt);
                derivative[joint] = jointMotion[joint, 1];
                gain[joint] = Convert.ToInt32((proportional[joint] + (ki[joint] * integral[joint]) - derivative[joint]) * 1000.0 * (_stictComp * kt[joint]));
                //gain[joint] = Convert.ToInt32(proportional[joint] + derivative[joint]);
                //gain[joint] = Convert.ToInt32(proportional[joint]);
                if (gain[joint] > maxGain[joint])
                    gain[joint] = maxGain[joint];
                if (gain[joint] < -maxGain[joint])
                    gain[joint] = -maxGain[joint];
                PWM = holdPWM[joint] + (jointDir[joint] * gain[joint]);
                PWMarray[joint] = PWM;
                //if (joint == tuningJoint)
                    //Debug.Write(String.Format("|J{7}| D{0,8:N2} E{1,6:N2} p{2,8:N2} i{3,8:N3} d{4,7:N3} g{5,4:G} PWM{6}", DesiredPos.getJoint(joint), ErrorPos.getJoint(joint), proportional[joint], ki[joint] * integral[joint], derivative[joint], gain[joint], PWMarray[joint], joint));
                //lastPos.Joint[joint] = currentPos.Joint[joint];
            }
            tempMotion = false;
            for(joint = 0; joint < 5; joint++)
            {
                
                if( !((Math.Abs(jointMotion[joint,0]) < 7.0) && (Math.Abs(jointMotion[joint,1]) < 0.5)) )
                    tempMotion = true;
                
            }
            InMotion = tempMotion;
            //Debug.WriteLine("PID InMotion:" + InMotion);
            //Debug.WriteLine("");
            LastPos = tempPos;
            return true;
        }
        public bool writeTorque()
        {
            lock (thisLock)
            {
                controllerObject.MoveTorque(PWMarray, USBC.TotalAxes);
            }
            return true;
        }
        public bool loopDelay()
        {
            loopEndTime = DateTime.Now;
            loopDuration = loopEndTime - loopStartTime;
            if (loopDuration.TotalSeconds < defaultdt)
            {
                _sleeping = true;
                Thread.Sleep(Convert.ToInt32((defaultdt * 1000.0) - loopDuration.TotalMilliseconds));
                dt = defaultdt;
                _sleeping = false;
            }
            else
            {
                dt = loopDuration.TotalSeconds;
            }
            //Debug.Write(String.Format("|{0,3:N1}Hz PID ", (1.0 / dt)));
            if (((defaultdt * 1000.0) - loopDuration.TotalMilliseconds) < 0.0)
            {
                //Debug.WriteLine(String.Format("lag {0,3:N3}ms *********NEGATIVE*******", (defaultdt * 1000.0) - loopDuration.TotalMilliseconds));
            }
            else
            {
                //Debug.WriteLine(String.Format("latency {0,3:N3}ms", 20.0 - loopDuration.TotalMilliseconds));
            }
           

            return true;
        }

        public double getDt()
        {
            return dt;
        }

       
    }
}
